﻿using System;
using System.Linq;
using System.ComponentModel;
using System.Windows.Media.Media3D;
using Kinect.KinectMouseControl;
using Microsoft.Research.Kinect.Nui;
using System.Windows.Media.Imaging;
//using System.Reactive.Concurrency;
//using System.Reactive.Threading;


/*
 * List of things to do/think about
 * Done - gestures for using left hand to control mouse down
 * Done - a too close warning.  Calculate distance and suggest how far to move forward or back based on head joint
 *      - disable mouse interface of person is too close
 * Done - change right hand mouse push to not use relative with shoulder, but relative to screen
 * Done - keep track of skeleton number and use that number until its no longer available
 * - voice commands for mouse down, cancel, yes, no, etc.
 * Done - create relative mouse positioning
 * Done - show skeleton or video frame on screen for user feedback
 * 
*/

namespace Kinban.ViewModels
{
    public class KinectMouseViewModel : INotifyPropertyChanged, IDisposable, IMouseController
    {
        private IMouseController _mouseController;
        private KinectFramesPerSecondObserver _framePerSecondObserver;
        private SkeletonTracker _tooCloseTracker;
        private IKinect _kinect;
        private IEventPublisher<SkeletonData> _skeletonDataPublisher;
        private IEventPublisher<BitmapSource> _bitmapSourcePublisher;
        private IEventPublisher<BitmapSource> _depthBitmapSourcePublisher;
        private IEventPublisher<bool> _mouseButtonDownGesture;
        private IEventPublisher<Point3D> _mousePositionGesture;
        private AbsoluteMouseTranslation _absoluteMouse;

        private const int Screen = 0;
        private const float MaxDistanceDifference = 0.5f;
        private const float PreferredDistance = 2.1f;
        private const float MouseDownDistance = 0.5f;
        private const int MouseMoveRefreshRateMilliseconds = 30;
        private const int ImageFrameRateMilliseconds = 90;
        private const float VelocityDeadZone = .1f;

        private const float MouseWindowTop = .3f;
        private const float MouseWindowBottom = -.3f;

        private const float MouseWindowWidth = .4f;

        private bool ShowTooCloseWarning = false;
        private bool ShowSkeletonFramePerSecondCounter = false;
        private bool ShowRightSideTrackingPoints = false;

        private Func<float, float> VelocityScalingFunction = (velocity) => { return Convert.ToSingle(System.Math.Pow(velocity, 3) * 500); };

        public KinectMouseViewModel()
        {
            InitializeProperties();

            _kinect = new KinectObserverCore(false, true);


            // depth image
            {
                _depthBitmapSourcePublisher = new DepthFrameToBitmapSourceConverter(_kinect, ImageFrameRateMilliseconds);
                _depthBitmapSourcePublisher.EventObservable.Subscribe(x => DepthImage = x);
            }

            // video image 
            {
                _bitmapSourcePublisher = new ImageFrameToBitmapSourceConverter(_kinect, ImageFrameRateMilliseconds);
                _bitmapSourcePublisher.EventObservable.Subscribe(x => VideoImage = x);
            }

            _skeletonDataPublisher = new ClosestTrackedSkeletonFilter(_kinect, MouseMoveRefreshRateMilliseconds);

            _mouseController = new AbsoluteMouseController();

            // wire up mouse button down gesture
            {
                _mouseButtonDownGesture = new LeftHandRaisedGesture(_skeletonDataPublisher);
                _mouseButtonDownGesture.EventObservable.Subscribe(SetMouseButtonDown);
            }

            // wire up mouse move gesture
            {

                _absoluteMouse = new AbsoluteMouseTranslation(
                    System.Windows.Forms.Screen.AllScreens[Screen].Bounds,
                    MouseWindowWidth,
                    MouseWindowTop,
                    MouseWindowBottom
                    );

                _mousePositionGesture = new RightHandPositionGesture(_skeletonDataPublisher);
                _mousePositionGesture.EventObservable.Subscribe( x => {
                    if (LiveMouse && System.Math.Abs(MoveBackMeters) < MaxDistanceDifference)
                    {
                        var screenPosition = _absoluteMouse.Translate(x);
                        SetMousePosition(screenPosition);
                    }
                });
            }

            if (ShowTooCloseWarning)
            {
                _tooCloseTracker = new SkeletonTracker(_kinect);
                _tooCloseTracker.EventObservable.Subscribe(SetDistanceMeter);
            }

            if (ShowSkeletonFramePerSecondCounter)
            {
                _framePerSecondObserver = new KinectFramesPerSecondObserver(_kinect);
                _framePerSecondObserver.EventObservable.Subscribe(x => FPS = x);
            }

            if (ShowRightSideTrackingPoints)
            {
                _skeletonDataPublisher.EventObservable.Subscribe(TrackedPointsDisplay);
            }
        }

        private void InitializeProperties()
        {
            RightHand = new Point3D();
            RightWrist = new Point3D();
            LiveMouse = true;
        }

        private void SetDistanceMeter(SkeletonLocations distance)
        {
            var closest = distance.Points.OrderBy(point => point.Z).FirstOrDefault();

            if (closest != null)
            {
                var expectedDistance = closest.Z - PreferredDistance;

                if (System.Math.Abs(expectedDistance) < MaxDistanceDifference)
                {
                    DistanceFeedback = "Just Right";
                }
                else if (expectedDistance < 0)
                {
                    DistanceFeedback = "Move Back";
                }
                else
                {
                    DistanceFeedback = "Move Closer";
                }
            }
            else
            {
                DistanceFeedback = string.Empty;
            }
        }

        private void TrackedPointsDisplay(SkeletonData skeletonData)
        {
            {
                var rightWristJoint = skeletonData.Joints[JointID.WristRight];
                if (rightWristJoint.TrackingState == JointTrackingState.Tracked)
                {
                    RightWrist = rightWristJoint.Position.ToPoint3D();
                }
            }

            {
                var rightHandJoint = skeletonData.Joints[JointID.HandRight];
                if (rightHandJoint.TrackingState == JointTrackingState.Tracked)
                {
                    RightHand = rightHandJoint.Position.ToPoint3D();
                }
            }

            {
                var rightFootJoint = skeletonData.Joints[JointID.FootRight];
                if (rightFootJoint.TrackingState == JointTrackingState.Tracked)
                {
                    RightFoot = rightFootJoint.Position.ToPoint3D();
                }
            }

            {
                var rightHipJoint = skeletonData.Joints[JointID.HipRight];
                if (rightHipJoint.TrackingState == JointTrackingState.Tracked)
                {
                    RightHip = rightHipJoint.Position.ToPoint3D();
                }
            }
        }

        #region Properties

        #region VideoImage (INotifyPropertyChanged Property)
        private BitmapSource _videoImage;

        public BitmapSource VideoImage
        {
            get { return _videoImage; }
            set
            {
                if (_videoImage != value)
                {
                    _videoImage = value;
                    RaisePropertyChanged("VideoImage");
                }
            }
        }
        #endregion

        #region DepthImage (INotifyPropertyChanged Property)
        private BitmapSource _depthImage;
        public BitmapSource DepthImage
        {
            get { return _depthImage; }
            set
            {
                if (_depthImage != value)
                {
                    _depthImage = value;
                    RaisePropertyChanged("DepthImage");
                }
            }
        }
        #endregion

        #region RightHand (INotifyPropertyChanged Property)
        private Point3D _rightHand;
        public Point3D RightHand
        {
            get { return _rightHand; }
            set
            {
                if (_rightHand != value)
                {
                    _rightHand = value;
                    RaisePropertyChanged("RightHand");
                }
            }
        }
        #endregion

        #region RightWrist (INotifyPropertyChanged Property)
        private Point3D _rightWrist;
        public Point3D RightWrist
        {
            get { return _rightWrist; }
            set
            {
                if (_rightWrist != value)
                {
                    _rightWrist = value;
                    RaisePropertyChanged("RightWrist");
                }
            }
        }
        #endregion

        #region RightFoot (INotifyPropertyChanged Property)
        private Point3D _rightFoot;
        public Point3D RightFoot
        {
            get { return _rightFoot; }
            set
            {
                if (_rightFoot != value)
                {
                    _rightFoot = value;
                    RaisePropertyChanged("RightFoot");
                }
            }
        }
        #endregion

        #region RightHip (INotifyPropertyChanged Property)
        private Point3D _rightHip;
        public Point3D RightHip
        {
            get { return _rightHip; }
            set
            {
                if (_rightHip != value)
                {
                    _rightHip = value;
                    RaisePropertyChanged("RightHip");
                }
            }
        }
        #endregion

        #region MouseDown (INotifyPropertyChanged Property)
        private bool _mouseDown;

        public bool MouseDown
        {
            get { return _mouseDown; }
            set
            {
                if (_mouseDown != value)
                {
                    _mouseDown = value;
                    RaisePropertyChanged("MouseDown");
                }
            }
        }
        #endregion

        #region FPS (INotifyPropertyChanged Property)
        private int _fps;

        public int FPS
        {
            get { return _fps; }
            set
            {
                if (_fps != value)
                {
                    _fps = value;
                    RaisePropertyChanged("FPS");
                }
            }
        }
        #endregion

        #region Information (INotifyPropertyChanged Property)
        private string _information;

        public string Information
        {
            get { return _information; }
            set
            {
                if (_information != value)
                {
                    _information = value;
                    RaisePropertyChanged("Information");
                }
            }
        }
        #endregion

        #region LiveMouse (INotifyPropertyChanged Property)
        private bool _liveMouse;

        public bool LiveMouse
        {
            get { return _liveMouse; }
            set
            {
                if (_liveMouse != value)
                {
                    _liveMouse = value;
                    RaisePropertyChanged("LiveMouse");
                }
            }
        }
        #endregion

        #region MoveBackMeters (INotifyPropertyChanged Property)
        private float _moveBackMeters;

        public float MoveBackMeters
        {
            get { return _moveBackMeters; }
            set
            {
                if (_moveBackMeters != value)
                {
                    _moveBackMeters = value;
                    RaisePropertyChanged("MoveBackMeters");
                }
            }
        }
        #endregion

        #region DistanceFeedback (INotifyPropertyChanged Property)
        private string _distanceFeedback;
        public string DistanceFeedback
        {
            get { return _distanceFeedback; }
            set
            {
                if (_distanceFeedback != value)
                {
                    _distanceFeedback = value;
                    RaisePropertyChanged("DistanceFeedback");
                }
            }
        }
        #endregion

        #endregion

        #region INotifyPropertyChanged values

        public event PropertyChangedEventHandler PropertyChanged;

        protected void RaisePropertyChanged(string propertyName)
        {
            if (PropertyChanged != null)
            {
                PropertyChanged(this, new PropertyChangedEventArgs(propertyName));
            }
        }
        #endregion

        internal void Initialize()
        {
            _mouseController.Initialize();
            try
            {
                _kinect.Initialize();

            }
            catch (Exception ex)
            {
                System.Windows.Forms.MessageBox.Show("Kinect unavailable.\r\n" + ex.ToString());
                //throw;
            }
        }

        #region IMouseController Members

        public void SetMousePosition(System.Drawing.Point mousePosition)
        {
                _mouseController.SetMousePosition(mousePosition);
        }

        void IMouseController.Initialize()
        {
            _mouseController.Initialize();
        }

        public void SetMouseButtonDown(bool mouseButtonDown)
        {
            this.MouseDown = mouseButtonDown;
            _mouseController.SetMouseButtonDown(mouseButtonDown);
        }

        #endregion

        #region IKinectStatus Members

        public int SkeletonFPS
        {
            get
            {
                return FPS;
            }
            set
            {
                FPS = value;
            }
        }

        #endregion

        #region IDisposable Members

        /// <summary>
        /// Internal variable which checks if Dispose has already been called
        /// </summary>
        private Boolean disposed;

        /// <summary>
        /// Releases unmanaged and - optionally - managed resources
        /// </summary>
        /// <param name="disposing"><c>true</c> to release both managed and unmanaged resources; <c>false</c> to release only unmanaged resources.</param>
        private void Dispose(Boolean disposing)
        {
            if (disposed)
            {
                return;
            }

            if (disposing)
            {
                //TODO: Managed cleanup code here, while managed refs still valid
                _kinect.Dispose();
            }
            //TODO: Unmanaged cleanup code here

            disposed = true;
        }

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        public void Dispose()
        {
            // Call the private Dispose(bool) helper and indicate 
            // that we are explicitly disposing
            this.Dispose(true);

            // Tell the garbage collector that the object doesn't require any
            // cleanup when collected since Dispose was called explicitly.
            GC.SuppressFinalize(this);
        }

        #endregion

    }
}

